/**
 * This file and all of its contents are designed and implemented by Luther
 * Tychonievich. If you desire to redistribute this file or any part thereof
 * in any way, please first obtain approval from the creator.
 *
 * tychonievich at gmail,com
 *
 */



#ifndef __VECMAT_H__
#define __VECMAT_H__

#include <iostream>
#include <math.h>
#include "typedmath.h"
#include <assert.h>



/**
 * The following tool is used for setting a chunk of memory. 
 */
template <typename N, int n>
struct memset_it {
	N* ptr;
	inline memset_it<N, n> (N* p) : ptr(p) {}
	inline memset_it<N, n-1>& operator ,(N x) { *ptr = x; ptr++; return *((memset_it<N, n-1>*)this); }
	template <typename T>
	inline memset_it<N, n-1>& operator ,(T x) { *ptr = N(x); ptr++; return *((memset_it<N, n-1>*)this); }
	inline operator int() { return n; }
};
template <typename N>
struct memset_it<N,0> {
	N* ptr;
	inline memset_it<N, 0> (N* p) : ptr(p) {}
	inline memset_it<N, 0>& operator ,(N x) { return *this; }
	template <typename T>
	inline memset_it<N, 0>& operator ,(T x) { return *this; }
	inline operator int() { return 0; }
};



template <typename N, int len> class vec;
template <typename N, int m, int n> class mat;

template <typename N, int m, int n> 
mat<N,m,n> _outer(const vec<N,m>&, const vec<N,n>&);



/**
 * A fixed-length mathematical vector class.
 * Putting the length as a template arg allows more compile-time type checking.
 * I suggest you would usually do something like 
 * 	typedef vec<float,3> vec3f;
 * in the beginning of you code so you don't have to worry about it.
 */
template <typename N, int len> 
class vec {
public:
	N data[len]; // public just to be friendly. Can also get it by "(N *)&x"
	
	// constructors:
	vec() { for (int i=0; i<len; i++) data[i] = 0; } // always zeroed by default
	vec(const N *a) { for (int i=0; i<len; i++) data[i] = a[i]; } // can be set to an array
	template <typename N2, int len2>
	vec(const vec<N2,len2>& o) { for (int i=0; i<len; i++) data[i] = ((i<len2)?(o[i]):(0)); }
	
	/// Although you cannot do "vec<N,7> x(1,2,3,4,5,6,7)" you can do "vec<N,7> x; x = 1,2,3,4,5,67;"
	/// DO NOT try to do "x = y = 1,2,3,4,5;" though--it won't work. Use "x = 1,2,3,4,5; y = x;" instead
	inline memset_it<N,len-1> operator = (N x) { data[0] = x; return memset_it<N,len-1>(data+1); }
	// The default copy constructor and assignment operators work fine.
	
	// Vector operators (pretty self-explanatory):
	vec<N,len> operator + (const vec<N,len>& o) const {
		vec<N,len> ans;
		for (int i=0; i<len; i++) {
			ans.data[i] = data[i] + o.data[i];
		}
		return ans;
	}
	vec<N,len> operator - (const vec<N,len>& o) const {
		vec<N,len> ans;
		for (int i=0; i<len; i++) {
			ans.data[i] = data[i] - o.data[i];
		}
		return ans;
	}
	vec<N,len>& operator += (const vec<N,len>& o) {
		for (int i=0; i<len; i++) {
			data[i] += o.data[i];
		}
		return *this;
	}
	vec<N,len>& operator -= (const vec<N,len>& o) {
		for (int i=0; i<len; i++) {
			data[i] -= o.data[i];
		}
		return *this;
	}
	vec<N,len> operator * (N s) const {
		vec<N,len> ans;
		for (int i=0; i<len; i++) {
			ans.data[i] = data[i]*s;
		}
		return ans;
	}
	vec<N,len>& operator *= (N s) {
		for (int i=0; i<len; i++) {
			data[i] *= s;
		}
		return *this;
	}
    vec<float,3> ToVec3()
    {
        vec<float,3> vec3; 
        if (len >= 3)
            vec3 = data[0], data[1], data[2];
        return vec3;
    }
	inline vec<N,len> operator / (N s) const { return this->operator*(1/s); }
	inline vec<N,len>& operator /= (N s) { return this->operator*=(1/s); }
	
	// vec::operator*(vec) ambiguous and thus not overridden. [x,y,z] * [p,q,r] could yield
	// -> [x*p, y*q, z*r] (element-wise product)
	// -> x*p + y*q + z*r (dot or inner product, == x*p' if x,p column vectors)
	// -> [x*p,x*q,x*r; y*p,y*q,y*r; z*p,z*q,z*r] (outer product, == x'*p if x,p column vectors)
	// -> det([e1,e2,e3; x,y,z; p,q,r]) (cross product)
	// Inner, Outer and Element-wise products are provided here as inner, outer, scale and iscale. 
	// Cross products are more involved, involve (n-1) operands, and are provided in matrixTools.h
	
	/// Inner Dot-product. vector inner vector = scalar
	N inner (const vec<N,len>& other) const {
		N ans = 0;
		for (int i=0; i<len; i++)
			ans += data[i]*other.data[i];
		return ans;
	}

    /// vector element wise times vector = vector
	vec<N,len> ewt (const vec<N,len>& other) const 
    {
		vec<N,len> ans(other);
		for (int i=0; i<len; i++)
			ans[i] *= data[i];
		return ans;
	}

	/// Cross Product
	vec<float,3> cross(const vec<float,3>& b) {
		vec<float,3> ans;
		ans[0] = data[1]*b[2] - b[1]*data[2];
		ans[1] = b[0]*data[2] - data[0]*b[2];
		ans[2] = data[0]*b[1] - b[0]*data[1];
		return ans;
	}

	/// Element-wise product or scaling. vector times vector = vector
	vec<N,len> scale(const vec<N,len>& o) const {
		vec<N,len> ans = *this;
		for (int i=0; i<len; i++) ans.data[i] *= o.data[i];
		return ans;
	}
	/// In-place element-wise product or scaling (e.g., *=). vector times vector = vector
	vec<N,len>& iscale(const vec<N,len>& o) {
		for (int i=0; i<len; i++) data[i] *= o.data[i];
		return *this;
	}
	/// Sometimes you wish a vec<n,4> was a vec<N,3>; just cast it as such.
	template <int len2>
	operator vec<N,len2>() const {
		vec<N,len2> ans;
		int mx = len>len2 ? len2 : len;
		for (int i=0; i<mx; i++) {
				ans[i] = data[i];
		}
		return ans;
	}
	
	/// Element access
	inline N& operator[] (int idx) { return data[idx]; }
	/// Element access
	inline N operator[] (int idx) const { return data[idx]; }
	
	/// Output in matlab/octave column-vector format: "[x; y; z; w]"
	friend std::ostream& operator <<(std::ostream& out, const vec<N,len>& x) {
		out << '[';
		for (int i=0; i<len; i++) {
			out << x[i];
			if (i < len-1)
				out << "; ";
		}
		return out<<"]";
	}
	
	/// [x;y;z].concat([r;p]) = [x;y;z;r;p]
	template <int r>
	vec<N, len+r> concat(const vec<N,r>& o) const {
		vec<N, len+r> ans;
		for (int i=0; i<len; i++)
			ans[i] = data[i];
		for (int i=0; i<r; i++)
			ans[len+i] = o[i];
		return ans;
	}
	/// The square of the l2-norm (aka Euclidean norm) of a vector, or x.inner(x)
	N l2sqr() const { 
		N ans = 0;
		for (int i=0; i<len; i++)
			ans += data[i]*data[i];
		return ans;
	}

    vec<float,len> normalize()
    {
        vec<float,len> n(*this);
        N norm = n.l2();
        for (int i=0; i<len; i++)
            n.data[i] = n.data[i] / norm;
        return n;
    }

	inline N l2() const { return SQRT(l2sqr()); }
	
	/// Sets all terms to uniform random numbers between -range and range
	void random(N range) {
		range *= 2;
		for (int i=0; i<len; i++) {
			data[i] = (N(rand())/RAND_MAX-0.5)*range;
		}
	}
	/// true if all elements literally zero; false otherwise. For magnitude checks, use (x < tol)
	bool operator !() const {
		for (int i=0; i<len; i++)
			if (data[i]) return false;
		return true;
	}
	/// true if all elements are less IN MAGNITUDE than val; false otherwise
	bool operator < (N val) const {
		for (int i=0; i<len; i++)
			if (ABS(data[i]) >= val) return false;
		return true;
	}

	/// Convert into an nxn matrix with the values of this vector on the main diagonal
	mat<N,len,len> asEye() const {
		mat<N,len,len> ans(*this);
		return ans;
	}
	/// Outer-product. vector times vector = matrix
	template<int n>
	mat<N,len,n> outer(const vec<N,n>& o) const {
		return _outer(*this,o);
	}

    vec<N,len> absoluteValue()
    {
        vec<N,len> ans = this->data;
        for (int i = 0; i < len; i++)
            ans.data[i] = abs(ans.data[i]);
        return ans;
    }
};




/**
 * A fixed-size mathematical matrix class.
 * Putting the dimensions as template args allows more compile-time type checking
 * This is a row-major storage method. OpenGL's 4x4 transformation matrices are column-major.
 * If you need to send a matrix object directly to OpenGL, use e.g.
 *  	mat<float,4,4> glm = m.transpose();
 *  	glLoadMatrixf(&(glm.data[0][0]));
 */
template <typename N, int m, int n>
class mat {
public:
	N data[m][n]; // public just to be friendly. Can also get it by "(N *)&x"
	
	/// Defaults to 1 on main diagonal, zero elsewhere
	mat() {
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				data[row][col] = (row==col);
	}
	/// Initialize elements to an array which is long enough and in row-major order.
	/// Note, if you want the matrix to alias the array, not copy it, you can do
	/// 	N array[n*m]; // or N array[m][n]; // or N *array = new N[n*m];
	/// 	mat<N,m,n>& x = *(mat<N,m,n>*)&(array[0]);
	/// but make sure your array stays allocated as long as the matrix does!
	mat(const N *a) {
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				data[row][col] = a[row*n+col];
	}
	/// create a diagonal matrix with zeros everywhere else
	template <int r>
	mat(const vec<N,r>& d) {
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				data[row][col] = 0;
		for (int i=0; i< CMP<m,CMP<n,r>::MIN>::MIN; i++)
				data[i][i] = d[i];
	}
	/// create from a list of values, in row-major order. 
	/// Thus, A = 1,2,3, 4,5,6, 7,8,9; creates [1, 2, 3; 4, 5, 6; 7, 8, 9]
	/// As with vectors, don't try A = B = 1,...
	inline memset_it<N,n*m-1> operator = (N x) { data[0][0] = x; return memset_it<N,n*m-1>(&data[0][0]+1); }
	/// create from a list of rows.
	/// Thus, A = [1;2;3], [4;5;6], [7;8;9]; creates [1, 2, 3; 4, 5, 6; 7, 8, 9]
	/// As with vectors, don't try A = B = x,...
	inline memset_it<vec<N,n>,m-1> operator = (vec<N,n>& x) 
	{ for (int i=0; i<n; i++) data[0][i] = x.data[i]; return memset_it<vec<N,n>,m-1>((vec<N,n>*)(&data[0][0]+n)); }
	
	/// Changes the contents of an already-created matrix
	void set(const N *a) {
		for (int row=0; row<m; row++)
			for (int col=!row; col<n; col++)
				data[row][col] = a[row*n+col];
	}
	/// Returns a matrix to the identity state
	void reset() {
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				data[row][col] = (row==col);
	}
	/// Sets a sub-matrix. 
	/// [1,2; 3,4].set(0,0,[7,8,9; a,b,c; d,e,f]) yields [7,8; a,b]
	/// [1,2; 3,4].set(-1,1,[7,8; 9,0]) yields [1,9; 3,4]
	template <int p, int q>
	void set(int r, int c, const mat<N,p,q>& o) {
		int rm=0,cm=0;
		if (r<0) { rm = -r; r = 0; }
		if (c<0) { cm = -c; c = 0; }
		for (int row=r, rowm=rm; row<m && rowm<p; row++, rowm++)
			for (int col=c, colm=cm; col<n && colm<q; col++, colm++)
				data[row][col] = o.data[rowm][colm];
	}
	/// Sets a sub-matrix. A.setTranspose(B) is equivalent to, but faster than, A.set(B.transpose())
	template <int p, int q>
	void setTranspose(int r, int c, const mat<N,p,q>& o) {
		int rm=0,cm=0;
		if (r<0) { rm = -r; r = 0; }
		if (c<0) { cm = -c; c = 0; }
		for (int row=r, rowm=rm; row<m && rowm<p; row++, rowm++)
			for (int col=c, colm=cm; col<n && colm<q; col++, colm++)
				data[row][col] = o.data[colm][rowm];
	}
	/// Because we are in row-major order, a column has to be a copy.
	vec<N,m> getCol(int c) const {
		vec<N,m> ans;
		for (int i=0; i<m; i++) ans[i] = data[i][c];
		return ans;
	}
	/// Accesses a row in place (unlike getCol method above).
	vec<N,n>& getRow(int r) { return *reinterpret_cast<vec<N,n>*>(&(data[r][0])); }
	/// Accesses and copies a row (like getCol does)
	const vec<N,n>& getRow(int r) const { return *reinterpret_cast<const vec<N,n>*>(&(data[r][0])); }
	
	/// element access
	inline N* operator[] (int r) {
		return data[r];
	}
	inline const N* operator[] (int r) const {
		return data[r];
	}
	
	// mathematical operations (pretty self-explanatory)
	mat<N,m,n> operator * (N s) const {
		mat<N,m,n> ans;
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				ans[row][col] = data[row][col] * s;
		return ans;
	}
	inline mat<N,m,n> operator / (N s) const {
		return (*this)*(1/s);
	}
	mat<N,m,n>& operator *= (N s) {
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				data[row][col] *= s;
		return *this;
	}
	inline mat<N,m,n>& operator /= (N s) {
		return (*this)*=(1/s);
	}
	mat<N,m,n> operator + (const mat<N,m,n>& o) const {
		mat<N,m,n> ans;
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				ans[row][col] = data[row][col] + o[row][col];
		return ans;
	}
	mat<N,m,n> operator - (const mat<N,m,n>& o) const {
		mat<N,m,n> ans;
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				ans[row][col] = data[row][col] - o[row][col];
		return ans;
	}
	mat<N,m,n>& operator += (const mat<N,m,n>& o) {
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				data[row][col] += o[row][col];
		return *this;
	}
	mat<N,m,n>& operator -= (const mat<N,m,n>& o) {
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				data[row][col] -= o[row][col];
		return *this;
	}
	/// matrix * matrix means matrix multiplication; no abiguity here
	template <int r>
	mat<N,m,r> operator * (const mat<N,n,r>& o) const {
		mat<N,m,r> ans;

    	for (int row = 0; row < m; ++row)
		{
			for (int col = 0; col < n; ++col)
			{
                ans[row][col] = 0;
                // row and column length are same
                for (int temp = 0; temp < m; ++temp)
                    ans[row][col] += data[row][temp] * o[temp][col];
			}
		}

		return ans;
	}
	// feel free to implement a *=, but I don't know any way faster than { *this = (*this * other); }
	/// matrix * vector assumes vector is a column vector
	vec<N, m> operator *(const vec<N, n>& x) const 
	{
		vec<N, m> ans;
		for (int row = 0; row < m; ++row)
		{
			N result = 0;
			for (int col = 0; col < n; ++col)
				result += data[row][col] * x[col];
			ans[row] = result;
		}
		return ans;
	}
	/// vector * matrix assumes vector is a row vector
	friend vec<N,n> operator *(const vec<N, m>& x, const mat<N,m,n>& A) {
		vec<N, m> ans;
		for (int row = 0; row < m; ++row)
		{
			N result = 0;
			for (int col = 0; col < n; ++col)
				result += data[row][col] * x[col];
			ans[row] = result;
		}
		return ans;
	}
	
	/// Again, matlab/octave syntax output; row-major, elements separated by ',' 
	/// and rows separated by ';', as [1,2;3,4] for a matrix with 2 in cell [0][1]
	friend std::ostream& operator <<(std::ostream& out, const mat<N,m,n>& x) {
		out << "\n[";
		for (int row=0; row<m; row++) {
			for (int col=0; col<n; col++) {
				out << x[row][col];
				if (col < n-1)
					out << ",";
			}
			if (row < m-1)
				out << ";\n ";
		}
		return out<<"]";
	}
	/// true if all elements literally 0; false otherwise. See also M < number
	bool operator !() const {
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				if (data[row][col]) return false;
		return true;
	}
	/// true if all elements less than val in magnitude; false otherwise
	bool operator < (N val) const {
		for (int row=0; row<m; row++)
			for (int col=0; col<n; col++)
				if (ABS(data[row][col]) >= val) return false;
		return true;
	}

	/// creates a new matrix which is a transpose of this one.
	mat<N,n,m> transpose() const {
		mat<N,n,m> ans;
		for (int r=0; r<m; r++)
			for (int c=0; c<n; c++)
				ans[c][r] = data[r][c];
		return ans;
	}
	/// the absolute value of the element of greatest magnitude
	N maxval() { 
		N ans = 0;
		for (int row=0; row<m; row++) 
			for (int col=0; col<n; col++)
				if (ABS(data[row][col]) > ans) ans = ABS(data[row][col]);
		return ans;
	}
	/// randomize all elements
	void random(N range) {
		range *= 2;
		for (int row=0; row<m; row++) {
			for (int col=0; col<n; col++)
				data[row][col] = (N(rand())/RAND_MAX-0.5)*range;
		}
	}
};

/// helper method for outer products, defined here for C++ dependency reasons.
template <typename N, int m, int n> 
mat<N,m,n> _outer(const vec<N,m>& a, const vec<N,n>& b) {
	mat<N,m,n> ans;
	for (int r=0; r<m; r++)
		for (int c=0; c<n; c++)
			ans[r][c] = a[r]*b[c];
	return ans;
}




// Note that we can do an inverse by many different methods; 
// That included below is just one I happened to have lying around that is fast for 4x4 matrices.
// It has the feature that it returns something finite even for singular matrices.

/// Compute the Moore-Penrose Pseudo-inverse analytically
template<typename N>
mat<N,2,2> fastpinv(const mat<N,2,2>& A) {
	if (!A) return A;
	mat<N,2,2> ans;
	N den = (A[0][0]*A[1][1]-A[0][1]*A[1][0]);
	N check = ABS(A[0][0]*A[1][1]) + ABS(A[0][1]*A[1][0]);
	if (check+den != check) { // non-singular; use inverse
		den = 1/den;
		ans = den*A[1][1],-den*A[0][1],-den*A[1][0],den*A[0][0];
		return ans;
	} else { // nearly singular; use pseudo-inverse
		den = 1.0/(A[0][0]*A[0][0] + A[0][1]*A[0][1] + A[1][0]*A[1][0] + A[1][1]*A[1][1]);
		ans = A[0][0]*den, A[1][0]*den, A[0][1]*den, A[1][1]*den;
		return ans;
	}
}
/// A fast 4x4 inversion based on block inversion techniques
template <typename N>
mat<N,4,4> fastinv(const mat<N,4,4>& m) {
	// separate m into block matrix [a, b; c, d]
	mat<N,4,4> ans; 
	mat<N,2,2> a,b,c,d;
	a.set(0,0,m); b.set(0,-2,m); c.set(-2,0,m); d.set(-2,-2,m);
	// Volker Strassen proved that [a, b; c, d]^-1 = [ia + ia*b*is*c*ia, -ia*b*is; -id*c*ia, id]
	// where "ia" is the inverse of "a" and "is" is the inverse of the Schur compliment of "m"
	a = fastpinv(a);
	d -= c*a*b; d = fastpinv(d);
	// slight algebra speeds the process
	c = (c*a) *= -1;
	b = (a*b) *= -1;
	ans.set(2,2,d);
	ans.set(0,2,b  = (b*d));
	ans.set(2,0,d*c);
	ans.set(0,0,a += (b*c));
	return ans;
}


#endif /*__VECMAT_H__*/
